#!/usr/bin/env python
# -*- coding: UTF-8 -*-

import rospy,tf
import os,json,cv2,time,ast
from v3_msgs.srv import *
from v3_msgs.msg import MarkerPoint,MapInfo,VirtualInfo,ElevatorInMapInfo,AutoGate,Elevator
from std_msgs.msg import String,Bool
from geometry_msgs.msg import Pose
from cln_msgs.msg import MarkerPointMsg
from cln_msgs.srv import (
    MapName,MapNameRequest,
    AllDockers,AllDockersRequest,
    GenAutoGatePath,GenAutoGatePathRequest,
    DiskTaskList,DiskTaskListRequest,
    ElevatorTransFloor,ElevatorTransFloorRequest
)
from v3_msgs.srv import (
    TaskSingleManager, TaskSingleManagerRequest,
    MapManagerRequest, MapManagerRequestRequest,
    PathGenerateManager,PathGenerateManagerRequest
)

# from tool import User_Data_Server
from tool import Log_Parser
log = Log_Parser()

class MapParser:
    def __init__(self):
        self.__node_name = "MapParser"
        # 初始化参数
        self.__maps_dir = rospy.get_param('/maps_dir','/home/aoting/user_config/map_database')
        # 初始化全局变量
        self.__map_name = ""
        self.__last_valid_name = "" #上次加载地图名称

        # 初始化ROS内容
        rospy.Subscriber("/map_name",String,callback=self.MapNameCallback)
        self.__map_manager_client = rospy.ServiceProxy("/map_manager_server", MapManagerRequest)
        self.__map_info_client = rospy.ServiceProxy("/map_info_server", MapInfoRequest)
        self.__robot_relocation_client = rospy.ServiceProxy("/robot_relocation_server", RobotRelocationRequest)
        self.__elevator_manager_client = rospy.ServiceProxy("/elevator_manager_server", ElevatorManager)
        self.__task_single_client = rospy.ServiceProxy("/task_manager_server",TaskSingleManager)
        # 兼容话题服务，后续删除
        self.__task_delete_mapname_Client = rospy.ServiceProxy("/task_delete_mapname",MapName)
        self.__task_update_mapname_Client = rospy.ServiceProxy("/task_update_mapname",MapName)
        self.__timertask_delete_mapname_client = rospy.ServiceProxy("/timertask_delete_mapname",MapName)
        self.__timertask_update_mapname_client = rospy.ServiceProxy("/timertask_update_mapname",MapName)
        self.__all_docker_query_client = rospy.ServiceProxy('/all_docker_query',AllDockers)
        self.__auto_gate_generate_client = rospy.ServiceProxy("/auto_gate_path_generate",GenAutoGatePath)
        self.__task_list_query_client = rospy.ServiceProxy("/task_list_query",DiskTaskList)
        self.__go_elevator_map_client = rospy.ServiceProxy('/trans_floor',ElevatorTransFloor)
        self.__p2p_path_client = rospy.ServiceProxy('/p2p_path_server',PathGenerateManager) #判断标记点是否在膨胀区域
    def InitParser(self, node_name):
        pass

    def ResDataInit(self):
        '''
            初始化返回数据
        '''
        return {
            "code": 0,
            "msg": "success",
        }
    
    def MapNameCallback(self, msg):
        # 获取地图名称
        self.__map_name = msg.data
        # 地图新建或更新时，当前地图名称为空
        # 仅在当前地图名称非空时，更新上次有效数据
        if not self.__map_name:
            self.__last_valid_name = self.__map_name
    
    def CreateMap(self, enable,  map_name = ""):
        '''
            开始/取消创建地图
        '''
        # 初始化
        ret_dat = self.ResDataInit()
        client_data = MapManagerRequestRequest()
        if enable:  #开始创建地图
            client_data.option = MapManagerRequestRequest.CREATE_MAP
        else:       #取消创建地图
            client_data.option = MapManagerRequestRequest.CANCEL_CREATE_MAP
        # 调用接口
        try:
            res_data = self.__map_manager_client.call(client_data)
            rospy.loginfo("[MapCreate] res_data: {}".format(res_data))
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[MapCreate] error: {}".format(error))
        finally:
            return ret_dat 
    
    def SaveMap(self, data):
        '''
            保存地图
        '''
        # 初始化
        ret_dat = self.ResDataInit()
        client_data = MapManagerRequestRequest()
        client_data.option = MapManagerRequestRequest.SAVE_MAP
        # 调用接口
        try:
            if not data : raise Exception("Request data error!")
            map_name = data["map_name"].encode('utf-8')
            client_data.map_base.name = map_name
            res_data = self.__map_manager_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message)
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[SaveMap] error: {}".format(error))        
        finally:
            return ret_dat 


    def GetMapName(self):
        '''
            获取当前地图名称
        '''
        ret_dat = self.ResDataInit()
        ret_dat["map_name"] = self.__map_name
        return ret_dat  

    def Time2Stamp(self, time_str):
        '''
            2022-03-18 10:54:00  -> 1647572040
        '''
        struct_time = time.strptime(time_str, '%Y-%m-%d %H:%M:%S')        
        timestamp = time.mktime(struct_time)
        # print(timestamp)
        return timestamp
    
    def GetMapList(self):
        '''
            获取地图列表
        '''
        ret_dat = self.ResDataInit()
        client_data = MapManagerRequestRequest()
        client_data.option = MapManagerRequestRequest.QUERY_MAP_LIST
        ret_dat["list"] = []
        try:
            res_data = self.__map_manager_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message)
            if res_data.list.list:
                for map_info in res_data.list.list:
                    ret_dat["list"].append(map_info.name)
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[GetMapList] error: {}".format(error))        
        finally:
            return ret_dat 
        
    
    def GetMapInfo(self, data):
        '''
            获取地图信息
        '''
        ret_dat = self.ResDataInit()
        client_data = MapInfoRequestRequest()
        client_data.option = MapInfoRequestRequest.QUERY_MAP_INFO
        try:
            if not data : raise Exception("Request data error!") 
            map_name = data["map_name"].encode('utf-8')
            client_data.map_info.base_info.name = map_name
            res_data = self.__map_info_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message)
            map_base_info = res_data.map_info.base_info
            ret_dat['position_x'] = map_base_info.metadata.origin.position.x
            ret_dat['position_y'] = map_base_info.metadata.origin.position.y
            ret_dat['height']= map_base_info.metadata.height
            ret_dat['width'] = map_base_info.metadata.width
            ret_dat['create_date'] = map_base_info.create_time
            ret_dat['area'] = map_base_info.area
            ret_dat["user_rotate_angle"] =  map_base_info.rotate_angle
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[GetMapInfo] error: {}".format(error))        
        finally:
            return ret_dat 

    def RenameMap(self, data):  
        '''
            地图重命名
        '''
        ret_dat = self.ResDataInit()
        try:
            if not data : raise Exception("Request data error!") 
            old_name = data["old_name"].encode('utf-8')
            new_name = data["new_name"].encode('utf-8')   
            req = MapManagerRequestRequest()
            req.map_base.name = old_name
            req.new_map_base.name = new_name
            req.option = req.RENAME_MAP
            res_data = self.__map_manager_client.call(req)
            if not res_data.response.success:
                raise Exception(res_data.response.message)
            # 同步任务操作
            req = TaskSingleManagerRequest()
            req.option = req.UPDATE_MAP_NAME
            req.task_single.task_base.map_base.name = old_name
            req.new_task_single.task_base.map_base.name = new_name   
            resp = self.__task_single_client.call(req)
            if not resp.response.success:
                raise Exception(resp.response.message)
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[RenameMap] error: {}".format(error))        
        finally:
            return ret_dat 
    
    def DeleteMap(self, data):
        '''
            删除地图
        '''
        ret_dat = self.ResDataInit()
        try:
            if not data : raise Exception("Request data error!")
            req = MapManagerRequestRequest()
            req.option = req.DELETE_MAP
            map_name = data["name"].encode('utf-8')
            req.map_base.name = map_name
            res_data = self.__map_manager_client.call(req)
            if not res_data.response.success:
                raise Exception(res_data.response.message)  
            # 同步任务操作
            req = TaskSingleManagerRequest()
            req.option = req.DELETE_TASK_WITH_MAP
            req.task_single.task_base.map_base.name = map_name
            resp = self.__task_single_client.call(req)
            if not resp.response.success:
                raise Exception(resp.response.message)
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[DeleteMap] error: {}".format(error))        
        finally:
            return ret_dat 
    
    def ResetMap(self, data):
        '''
            重置地图
        '''
        ret_dat = self.ResDataInit()
        try:
            pass
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[ResetMap] error: {}".format(error))        
        finally:
            return ret_dat 
    
    def CopyMap(self, data):
        '''
            复制地图
        '''
        ret_dat = self.ResDataInit()
        client_data = MapManagerRequestRequest()
        client_data.option = MapManagerRequestRequest.COPY_MAP
        try:
            if not data : raise Exception("Request data error!")
            if not data["map_name"] : raise Exception("Map name is empty!")
            if not data["map_name_new"] : raise Exception("New map name is empty!")
            if not data["floor"] : raise Exception("floor is empty!")
            map_name = data["map_name"].encode('utf-8')
            new_map_name = data["map_name_new"].encode('utf-8')
            floor = int(data["floor"])
            client_data.map_base.name = map_name
            client_data.new_map_base.name = new_map_name
            client_data.new_map_base.floor.floor = floor
            res_data = self.__map_manager_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message)     
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[CopyMap] error: {}".format(error))        
        finally:
            return ret_dat 
    
    def MapClearNoise(self, data):
        '''
            清除地图中噪点
        '''
        ret_dat = self.ResDataInit()
        client_data = MapManagerRequestRequest()
        client_data.option = MapManagerRequestRequest.MAP_FILTER
        try:
            client_data.map_base.name = self.__map_name
            res_data = self.__map_manager_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message)
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[MapClearNoise] error: {}".format(error))        
        finally:
            return ret_dat 

    def LoadMap(self, data):
        '''
            加载地图
        '''
        ret_dat = self.ResDataInit()
        client_data = MapManagerRequestRequest()
        client_data.option = MapManagerRequestRequest.LOAD_MAP
        try:
            if not data : raise Exception("Request data error!")
            map_name = data["name"].encode('utf-8')
            client_data.map_base.name = map_name
            res_data = self.__map_manager_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message)
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[LoadMap] error: {}".format(error))        
        finally:
            return ret_dat 
    
    def UpdateMap(self, enable):
        '''
            开始/取消更新地图
        '''
        # 初始化
        ret_dat = self.ResDataInit()
        client_data = MapManagerRequestRequest()
        if enable:  #开始更新地图
            client_data.option = MapManagerRequestRequest.UPDATE_MAP
        else:       #取消更新地图
            client_data.option = MapManagerRequestRequest.CANCEL_UPDATE_MAP
        # 调用接口
        try:
            res_data = self.__map_manager_client.call(client_data)
            rospy.loginfo("[UpdateMap] res_data: {}".format(res_data))
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[UpdateMap] error: {}".format(error))        
        finally:
            return ret_dat 
    
    def SaveUpdatedMap(self, data):
        ''' 
            保存更新后的地图
        '''
        ret_dat = self.ResDataInit()
        client_data = MapManagerRequestRequest()
        client_data.option = MapManagerRequestRequest.SAVE_UPDATE_MAP
        # 调用接口
        try:
            if not data : raise Exception("Request data error!")
            if not data["map_name"]: raise Exception("Map name is empty!")
            map_name = data["map_name"].encode('utf-8')
            client_data.map_base.name = map_name
            if data.has_key("map_name_new") and data["map_name_new"]:
                map_name_new = data["map_name_new"].encode('utf-8')
                client_data.new_map_base.name = map_name_new
            res_data = self.__map_manager_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message)
            
            # 待补全，更新后自动加载地图
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[SaveUpdatedMap] error: {}".format(error))        
        finally:
            return ret_dat 
    
    #---------------------------------#
    # 标记点操作
    #---------------------------------#

    def AddMarkerPoint(self, data):
        '''
            添加标记点
            tag 标记点类型 0充电桩，1停靠点，2电梯点，3,反光条，4梯空充电桩
            flag 1默认充电桩（停靠点），0普通充电桩（停靠点）
        '''
        ret_dat = self.ResDataInit()
        client_data = MapInfoRequestRequest()
        client_data.option = MapInfoRequestRequest.CREATE_MARKPOINT
        error_code = None
        try:
            if not data: raise Exception("Request data error!")
            is_pile = False
            # 解析数据
            for marker_point_item in data: 
                marker_point = MarkerPoint()
                pointofpath = PathGenerateManagerRequest()

                if "map_name" in marker_point_item:
                    client_data.map_info.base_info.name = marker_point_item["map_name"].encode('utf-8')

                if "tag_name" in marker_point_item:
                    marker_point.name = marker_point_item["tag_name"].encode('utf-8')

                if "tag" in marker_point_item:
                    tag = str(marker_point_item["tag"])
                    if tag == '0':
                        marker_point.type = MarkerPoint.CHARGING_PILE
                        is_pile = True
                    if tag == '1':
                        marker_point.type = MarkerPoint.STOP
                    if tag == '2':
                        # marker_point.type = MarkerPoint.STOP
                        pass
                    if tag == '3':
                        marker_point.type = MarkerPoint.REFLECTOR
                    if tag == '4':
                        marker_point.type = MarkerPoint.CHARGING_PILE_ELEVATOR
                


                if "is_default" in marker_point_item:  
                    marker_point.is_default = str(marker_point_item["is_default"]) == '1'

                if "position" in marker_point_item:
                    position = marker_point_item["position"]
                    pointofpath.option = PathGenerateManagerRequest.POSITION_EFFECTIVE
                    pointofpath.path_gen.end.position.x=float(position[0])
                    pointofpath.path_gen.end.position.y=float(position[1])
                    if is_pile:
                        pointofpath.path_gen.is_pile=True
                    res = self.__p2p_path_client.call(pointofpath)
                    if not res.response.success:
                        ret_dat["code"] = res.response.error_code.error_code
                        ret_dat["msg"]= str(res.response.error_code.error_msg) 
                        return ret_dat
                    marker_point.pose.position.x = float(position[0])
                    marker_point.pose.position.y = float(position[1])
                    q = tf.transformations.quaternion_from_euler(0, 0, float(position[2]))
                    marker_point.pose.orientation.x = q[0]
                    marker_point.pose.orientation.y = q[1]
                    marker_point.pose.orientation.z = q[2]
                    marker_point.pose.orientation.w = q[3]

                # 填充数据
                client_data.map_info.marker_point_list.append(marker_point)
            # 客户端请求
            res_data = self.__map_info_client.call(client_data)
            if not res_data.response.success:
                error_code = res_data.response.error_code.error_code
                raise Exception(res_data.response.message)  

        except Exception as error:
            if error_code and error_code > 0:
                ret_dat["code"]= error_code
            else:
                ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[AddMarkerPoint] error: {}".format(error))        
        finally:
            return ret_dat 
    
    def DeleteMarkerPoint(self, data):
        '''
            删除标记点
        '''
        ret_dat = self.ResDataInit()
        client_data = MapInfoRequestRequest()
        client_data.option = MapInfoRequestRequest.DELETE_MARKPOINT
        try:
            if not data: raise Exception("Request data error!")
            # 解析数据
            for marker_point_item in data: 
                marker_point = MarkerPoint()

                if "map_name" in marker_point_item:
                    client_data.map_info.base_info.name = marker_point_item["map_name"].encode('utf-8')

                if "tag_name" in marker_point_item:
                    marker_point.name = marker_point_item["tag_name"].encode('utf-8')

                if "tag" in marker_point_item:
                    tag = str(marker_point_item["tag"])
                    if tag == '0':
                        marker_point.type = MarkerPoint.CHARGING_PILE
                    if tag == '1':
                        marker_point.type = MarkerPoint.STOP
                    if tag == '2':
                        # marker_point.type = MarkerPoint.STOP
                        pass
                    if tag == '3':
                        marker_point.type = MarkerPoint.REFLECTOR
                    if tag == '4':
                        marker_point.type = MarkerPoint.CHARGING_PILE_ELEVATOR                

                # 填充数据
                client_data.map_info.marker_point_list.append(marker_point)
            # 客户端请求
            res_data = self.__map_info_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message) 
            
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[DeleteMarkerPoint] error: {}".format(error))        
        finally:
            return ret_dat 
    
    def UpdateMarkerPoint(self, data):
        '''
            更新标记点
        '''
        ret_dat = self.ResDataInit()
        client_data = MapInfoRequestRequest()
        client_data.option = MapInfoRequestRequest.UPDATE_MARKPOINT
        try:
            map_info = MapInfo()
            new_map_info = MapInfo()
            if not data: raise Exception("Request data error!")

            # 解析数据
            for marker_point_item in data: 
                marker_point = MarkerPoint()

                if "map_name" in marker_point_item:
                    map_info.base_info.name = marker_point_item["map_name"].encode('utf-8')
                    new_map_info.base_info.name = marker_point_item["map_name"].encode('utf-8')

                if "tag_name" in marker_point_item:
                    marker_point.name = marker_point_item["tag_name"].encode('utf-8')
                    print("tag_name: ",marker_point.name)

                if "tag" in marker_point_item:
                    tag = str(marker_point_item["tag"])
                    if tag == '0':
                        marker_point.type = MarkerPoint.CHARGING_PILE
                    if tag == '1':
                        marker_point.type = MarkerPoint.STOP
                    if tag == '2':
                        # marker_point.type = MarkerPoint.STOP
                        pass
                    if tag == '3':
                        marker_point.type = MarkerPoint.REFLECTOR
                    if tag == '4':
                        marker_point.type = MarkerPoint.CHARGING_PILE_ELEVATOR
                
                if "is_default" in marker_point_item:  
                    marker_point.is_default = str(marker_point_item["is_default"]) == '1'
                
                # 更新数据
                map_info.marker_point_list.append(marker_point)

                if "new_tag_name" in marker_point_item:
                    new_marker_point = MarkerPoint()
                    new_marker_point.name = marker_point_item["new_tag_name"].encode('utf-8')
                    print("new_tag_name: ",new_marker_point.name)
                    new_marker_point.type = marker_point.type
                    new_marker_point.is_default = marker_point.is_default
                    new_map_info.marker_point_list.append(new_marker_point)
                else:
                    new_map_info.marker_point_list.append(marker_point)
            
            # 客户端请求
            client_data.map_info = map_info
            client_data.new_map_info = new_map_info
            # print("client_data: ",client_data)
            res_data = self.__map_info_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message)              
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[UpdateMarkerPoint] error: {}".format(error))        
        finally:
            return ret_dat 
    
    def MarkerPointList(self, data):
        '''
            获取标记点列表
        '''
        ret_dat = self.ResDataInit()
        client_data = MapInfoRequestRequest()
        client_data.option = MapInfoRequestRequest.QUERY_MAP_INFO
        try:
            if not data: raise Exception("Request data error!")
            map_name = data["map_name"].encode('utf-8')
            client_data.map_info.base_info.name = map_name
            # 客户端请求
            res_data = self.__map_info_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message) 
            ret_dat["data"] = []
            for marker_point_item in res_data.map_info.marker_point_list:
                marker_point_dict = {}
                marker_point_dict["map_name"] = client_data.map_info.base_info.name
                marker_point_dict["is_default"] = int(marker_point_item.is_default)
                marker_point_dict["tag_name"] = marker_point_item.name
                r,p,y = tf.transformations.euler_from_quaternion([
                        marker_point_item.pose.orientation.x, 
                        marker_point_item.pose.orientation.y, 
                        marker_point_item.pose.orientation.z,
                        marker_point_item.pose.orientation.w
                ])
                marker_point_dict["position"] = [
                        marker_point_item.pose.position.x,
                        marker_point_item.pose.position.y,
                        y
                ]
                marker_point_dict["tag"] = 0
                if marker_point_item.type == MarkerPoint.CHARGING_PILE:
                    marker_point_dict["tag"] = 0
                if marker_point_item.type == MarkerPoint.STOP:
                    marker_point_dict["tag"] = 1
                # if marker_point_item.type == MarkerPoint.CHARGING_PILE:
                    # marker_point_dict["tag"] = 2
                if marker_point_item.type == MarkerPoint.REFLECTOR:
                    marker_point_dict["tag"] = 3
                if marker_point_item.type == MarkerPoint.CHARGING_PILE_ELEVATOR:
                    marker_point_dict["tag"] = 4

                ret_dat["data"].append(marker_point_dict)

        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[MarkerPointList] error: {}".format(error))        
        finally:
            return ret_dat 
    
    #---------------------------------#
    # 虚拟数据操作
    #---------------------------------#  
    def GetVirtualLines(self, data):
        '''
            获取虚拟墙
        '''
        ret_dat = self.ResDataInit()
        client_data = MapInfoRequestRequest()
        client_data.option = MapInfoRequestRequest.QUERY_MAP_INFO
        try:
            if not data: raise Exception("Request data error!")
            map_name = data["map_name"].encode('utf-8')
            client_data.map_info.base_info.name = map_name
            # 客户端请求
            res_data = self.__map_info_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message) 
            virtual_data_info = []
            for virtual_data in res_data.map_info.virtual_info_list:
                # 虚拟墙数据
                if virtual_data.virtual_data_type == VirtualInfo.VIRTUAL_WALL:
                    virtual_wall = {}
                    virtual_wall["type"] = 0
                    if virtual_data.points_type == VirtualInfo.LINE:
                        line = {}
                        line["sx"] = virtual_data.line.start_pose.position.x
                        line["sy"] = virtual_data.line.start_pose.position.y
                        line["ex"] = virtual_data.line.end_pose.position.x
                        line["ey"] = virtual_data.line.end_pose.position.y
                        virtual_wall["lines"]=line
                    if virtual_data.points_type == VirtualInfo.CIRCLE:
                        circle = {}
                        circle["cx"] = virtual_data.circle.center_pose.position.x
                        circle["cy"] = virtual_data.circle.center_pose.position.y
                        circle["radius"] = virtual_data.circle.radius
                        virtual_wall["circle"]=circle  
                    virtual_data_info.append(virtual_wall)
            ret_dat["virtual_wall"]=virtual_data_info                             
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[GetVirtualLines] error: {}".format(error))        
        finally:
            return ret_dat 
    
    def UpdateVirtualLines(self, data, png_str):
        '''
            更新虚拟墙
        '''
        ret_dat = self.ResDataInit()
        client_data = MapInfoRequestRequest()
        client_data.option = MapInfoRequestRequest.UPDATE_VIRTUAL_WALL
        try:
            print("UpdateVirtualLines data: ", data) 
            if not data: raise Exception("Request data error!")
            if "map_name" not in data: raise Exception("Request data error!")
            if "virlines_wall" not in data: raise Exception("Request data error!")
            map_name = data["map_name"].encode('utf-8')
            client_data.map_info.base_info.name = map_name
            client_data.map_png_str = png_str
            # 虚拟墙数据解析
            req_virtual_data = ast.literal_eval(data["virlines_wall"])
            
            if req_virtual_data.has_key("lines"):
                for line in req_virtual_data["lines"]:
                    virtual_data = VirtualInfo()
                    virtual_data.virtual_data_type = VirtualInfo.VIRTUAL_WALL
                    virtual_data.points_type = VirtualInfo.LINE
                    start_pose = Pose()
                    end_pose = Pose()
                    start_pose.position.x = line["sx"]
                    start_pose.position.y = line["sy"]
                    end_pose.position.x = line["ex"]
                    end_pose.position.y = line["ey"]
                    virtual_data.line.start_pose = start_pose
                    virtual_data.line.end_pose = end_pose
                    client_data.map_info.virtual_info_list.append(virtual_data)  
            if req_virtual_data.has_key("circle"):
                for circle in req_virtual_data["circle"]:
                    virtual_data = VirtualInfo()
                    virtual_data.virtual_data_type = VirtualInfo.VIRTUAL_WALL
                    virtual_data.points_type = VirtualInfo.CIRCLE
                    center_pose = Pose()
                    center_pose.position.x = circle["cx"]
                    center_pose.position.y = circle["cy"]
                    virtual_data.circle.radius = circle["radius"]
                    virtual_data.circle.center_pose = center_pose
                    client_data.map_info.virtual_info_list.append(virtual_data)  
            
            # 客户端请求
            res_data = self.__map_info_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message) 
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[UpdateVirtualLines] error: {}".format(error))        
        finally:
            return ret_dat 
    

    def RibbonList(self, data):
        '''
            获取虚拟墙列表 待删除
        '''
        ret_dat = self.ResDataInit()
        try:
            pass
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[RibbonList] error: {}".format(error))        
        finally:
            return ret_dat 
    
    def RibbonEdit(self, data):
        '''
            编辑虚拟墙 待删除
        '''
        ret_dat = self.ResDataInit()
        try:
            pass
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[RibbonEdit] error: {}".format(error))        
        finally:
            return ret_dat 
    
    #---------------------------------#
    # 电梯位姿操作
    #---------------------------------#
    def AddElevatorMarkerPoint(self, data):
        '''
            添加电梯标记点
        '''
        ret_dat = self.ResDataInit()
        client_data = MapInfoRequestRequest()
        try:
            if not data: raise Exception("Request data error!")
            map_name = data["map_name"].encode('utf-8')
            floor = int(data["floor"])

            ## 兼容操作（先查询当前梯控设备信息，进行绑定），后续多梯控设备删除删除
            elevator_client_data = ElevatorManagerRequest()
            elevator_client_data.option = ElevatorManagerRequest.QUERY_ALL_ELEVATOR_INFO
            res_data = self.__elevator_manager_client.call(elevator_client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message) 
            if len(res_data.elevator_list) !=1:     #判断是否已绑定电梯，后续支持多梯控设备后兼容删除
                raise Exception("can't get elevator_list.")
            elevator_info = res_data.elevator_list[0]

            # 请求添加标记点
            client_data.map_info.base_info.name = map_name
            client_data.map_info.base_info.floor.floor = floor
            client_data.option = MapInfoRequestRequest.CREATE_ELEVATORPOSE
            elevator_pose_in_map = ElevatorInMapInfo()
            elevator_pose_in_map.elevator = elevator_info
            elevator_pose_in_map.pose.position.x = data["elevator_point"]["x"]
            elevator_pose_in_map.pose.position.y = data["elevator_point"]["y"]
            q = tf.transformations.quaternion_from_euler(0, 0, float(data["elevator_point"]["yaw"]))
            elevator_pose_in_map.pose.orientation.x = q[0]
            elevator_pose_in_map.pose.orientation.y = q[1]
            elevator_pose_in_map.pose.orientation.z = q[2]
            elevator_pose_in_map.pose.orientation.w = q[3]
            for rect_one in data["elevator_rect"]:
                pose_msg = Pose()
                pose_msg.position.x = rect_one["x"]
                pose_msg.position.y = rect_one["y"]
                elevator_pose_in_map.elevator_polygon.poses.append(pose_msg)
            # 1.8m 电梯内门到滑行点的距离
            if data.has_key("taxiing_distance"): 
                elevator_pose_in_map.taxiing_distance = float(data["taxiing_distance"])
            client_data.map_info.elevator_poses_list.append(elevator_pose_in_map)
            # 客户端请求
            res_data = self.__map_info_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message)             
            
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[AddElevatorMarkerPoint] error: {}".format(error))        
        finally:
            return ret_dat 
    
    def DeleteElevatorMarkerPoint(self, data):
        '''
            删除电梯标记点
        '''
        ret_dat = self.ResDataInit()
        client_data = MapInfoRequestRequest()
        try:
            if not data: raise Exception("Request data error!")
            map_name = data["map_name"].encode('utf-8')

            ## 兼容操作（先查询当前梯控设备信息，进行绑定），后续多梯控设备删除删除
            elevator_client_data = ElevatorManagerRequest()
            elevator_client_data.option = ElevatorManagerRequest.QUERY_ALL_ELEVATOR_INFO
            res_data = self.__elevator_manager_client.call(elevator_client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message) 
            if len(res_data.elevator_list) !=1:     #判断是否已绑定电梯，后续支持多梯控设备后兼容删除
                raise Exception("can't get elevator_list.")
            elevator_info = res_data.elevator_list[0]

            # 客户端请求
            client_data.map_info.base_info.name = map_name
            client_data.option = MapInfoRequestRequest.DELETE_ELEVATORPOSE
            elevator_pose_in_map = ElevatorInMapInfo()
            elevator_pose_in_map.elevator = elevator_info
            client_data.map_info.elevator_poses_list.append(elevator_pose_in_map)
            res_data = self.__map_info_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message)  
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[DeleteElevatorMarkerPoint] error: {}".format(error))        
        finally:
            return ret_dat 
    
    def UpdateElevatorMarkerPoint(self, data):
        '''
            更新电梯标记点
        '''
        ret_dat = self.ResDataInit()
        client_data = MapInfoRequestRequest()
        try:
            if not data: raise Exception("Request data error!")
            map_name = data["map_name"].encode('utf-8')
            floor = int(data["floor"])
            
            # ## 兼容操作（先查询当前梯控设备信息，进行绑定），后续多梯控设备删除删除
            # client_data.option = MapInfoRequestRequest.QUERY_ELEVATOR_DEVICE_INFO
            # res_data = self.__map_info_client.call(client_data)
            # if not res_data.response.success:
            #     raise Exception(res_data.response.message) 
            # if len(res_data.elevator_list) !=1:
            #     raise Exception("can't get elevator_list.")
            # elevator_info = res_data.elevator_list[0] 
            # elevator_pose_in_map = ElevatorInMapInfo()
            # elevator_pose_in_map.elevator = elevator_info
            
            # 当前节点APP只支持修改楼层，此处做兼容操作
            client_data.option = MapInfoRequestRequest.UPDATE_ELEVATORPOSE
            client_data.map_info.base_info.name = map_name
            # client_data.map_info.elevator_poses_list.append(elevator_pose_in_map)
            client_data.new_map_info.base_info.name = map_name
            client_data.new_map_info.base_info.floor.floor = floor
            # client_data.new_map_info.elevator_poses_list.append(elevator_pose_in_map)
            res_data = self.__map_info_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message) 

        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[UpdateElevatorMarkerPoint] error: {}".format(error))        
        finally:
            return ret_dat 
    
    def ElevatorMarkerPointList(self, data):
        '''
            获取电梯标记点列表(当前地图)
        '''
        ret_dat = self.ResDataInit()
        client_data = MapInfoRequestRequest()
        client_data.option = MapInfoRequestRequest.QUERY_MAP_INFO
        try:
            # if not data: raise Exception("Request data error!")
            # map_name = data["map_name"].encode('utf-8')
            # client_data.map_info.base_info.name = map_name
            client_data.map_info.base_info.name = self.__map_name
            # 客户端请求
            res_data = self.__map_info_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message) 
            # 解析电梯标记点列表数据
            elevator_items = []
            for elevator_poses_item in res_data.map_info.elevator_poses_list:
                elevator_item = {}
                elevator_item["map_name"] = res_data.map_info.base_info.name
                elevator_item["floor"] = res_data.map_info.base_info.floor.floor
                # elevator_point
                elevator_point = {}
                elevator_point["x"] = elevator_poses_item.pose.position.x
                elevator_point["y"] = elevator_poses_item.pose.position.y
                r,p,y = tf.transformations.euler_from_quaternion([
                        elevator_poses_item.pose.orientation.x, 
                        elevator_poses_item.pose.orientation.y, 
                        elevator_poses_item.pose.orientation.z,
                        elevator_poses_item.pose.orientation.w
                ])
                elevator_point["yaw"] = y 
                elevator_item["elevator_point"] = elevator_point
                # elevator_rect
                elevator_rect = []
                for rect_pose in elevator_poses_item.elevator_polygon.poses:
                    p={}
                    p["x"]=rect_pose.position.x
                    p["y"]=rect_pose.position.y
                    elevator_rect.append(p)
                    elevator_item["elevator_rect"] = elevator_rect   
                # taxiing_distance 
                p={}
                p["x"]=elevator_poses_item.taxiing_distance
                p["y"]=255   
                elevator_item["elevator_rect"] = elevator_rect  
                elevator_items.append(elevator_item)          
            ret_dat["data"] = elevator_items
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[ElevatorMarkerPointList] error: {}".format(error))        
        finally:
            return ret_dat 
    
    def RobotMapsFlagList(self):
        '''
            获取机器所有地图的充电桩标记点列表 
        '''
        ret_dat = self.ResDataInit()
        client_data = AllDockersRequest()
        try:
            # 使用旧版本兼容服务，后续待删除
            res_data = self.__all_docker_query_client(client_data)
            if res_data.success:
                raise Exception(res_data.message)

            elevator_dockers_list = []
            maps_flags_list_duplication = [] # 数据内容可能重复
            maps_flags_list = []
            if res_data.marker_points:# 数据存在       
                for marker_point in res_data.marker_points: # 构建地图充电桩列表
                    marker_item = {}
                    marker_item["map_name"] = marker_point.map_name
                    marker_item["building"] = marker_point.building
                    marker_item["floor"] = marker_point.floor
                    maps_flags_list_duplication.append(marker_item)

                # 去除重复
                for maps_flags in maps_flags_list_duplication:
                    if maps_flags not in maps_flags_list:
                        maps_flags_list.append(maps_flags)
                
                # 梯控充电桩列表，
                for marker_point in res_data.marker_points:

                    if marker_point.tag == str(MarkerPointMsg().GLOBALDOCKER): #梯控充电桩
                        elevator_dockers_item = {}
                        elevator_dockers_item["map_name"] = marker_point.map_name
                        elevator_dockers_item["flag_name"] = marker_point.name
                        elevator_dockers_list.append(elevator_dockers_item)

                # 向地图充电桩列表填充数据
                for maps_flags_list_one in maps_flags_list:
                    flag_list = []
                    for marker_point in res_data.marker_points:
                    
                        if maps_flags_list_one["map_name"] == marker_point.map_name:

                            flag_item = {}
                            flag_item["flag_name"] = marker_point.name
                            flag_item["flag_type"] = int(marker_point.tag)
                    
                            rot = [marker_point.pose.orientation.x,
                            marker_point.pose.orientation.y,
                            marker_point.pose.orientation.z,
                            marker_point.pose.orientation.w]
                            (roll,pitch,yaw)=tf.transformations.euler_from_quaternion(rot)
                            
                            flag_item["position"] = [marker_point.pose.position.x, marker_point.pose.position.y,yaw]
                            flag_list.append(flag_item)
                            # flag_list_item = {}
                            maps_flags_list_one["flag_list"] = flag_list

                            # maps_flags_list.append(flag_list_item)
            
            data_item = {}
            data_item["elevator_dockers"] = elevator_dockers_list
            data_item["maps_flags_list"] = maps_flags_list

            ret_dat["data"] = data_item
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[RobotMapsFlagList] error: {}".format(error))        
        finally:
            return ret_dat 
    
    def ElevatorDockerSet(self, data):
        '''
            设置机器人梯控充电桩位置
        '''
        ret_dat = self.ResDataInit()
        try:
            if not data: raise Exception("Request data error!")
            map_name = data["map_name"].encode('utf-8')
            flag_name = data["flag_name"].encode('utf-8')   

            # 查询当前地图信息
            client_data = MapInfoRequestRequest()
            client_data.option = MapInfoRequestRequest.QUERY_MAP_INFO
            client_data.map_info.base_info.name = map_name
            # 客户端请求
            res_data = self.__map_info_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message) 
            # 更新标记点信息
            new_client_data = MapInfoRequestRequest()
            new_client_data.map_info.base_info = res_data.map_info.base_info
            for marker_point_item in res_data.map_info.marker_point_list:
                point_msg = MarkerPoint()
                point_msg = marker_point_item
                # 旧有梯控充电桩信息变更
                if marker_point_item.type == marker_point_item.CHARGING_PILE_ELEVATOR and \
                    marker_point_item.name != flag_name:
                    point_msg.type = marker_point_item.CHARGING_PILE
                    new_client_data.map_info.marker_point_list.append(marker_point_item)
                    new_client_data.new_map_info.marker_point_list.append(point_msg)
                # 新增梯控充电桩信息
                if marker_point_item.name == flag_name:
                    point_msg.type = marker_point_item.CHARGING_PILE_ELEVATOR
                    new_client_data.map_info.marker_point_list.append(marker_point_item)
                    new_client_data.new_map_info.marker_point_list.append(point_msg)       
            # 客户端请求
            new_client_data.option = MapInfoRequestRequest.UPDATE_MARKPOINT
            res_data = self.__map_info_client.call(new_client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message) 

        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[ElevatorDockerSet] error: {}".format(error))        
        finally:
            return ret_dat 
    
    def GoElevtorMap(self, data):
        '''
            机器人前往指定地图楼层  兼容
        '''
        ret_dat = self.ResDataInit()
        try:
            if not data: raise Exception("Request data error!")
            map_name = data["map_name"].encode('utf-8')
            elevator_req = ElevatorTransFloorRequest()
            elevator_req.current_map_name = self.__map_name
            elevator_req.target_map_name = map_name
            
            res_data = self.__go_elevator_map_client(elevator_req)
            if res_data.ret: 
                err_msg = "[MapParser]Failed to go elevator map: {} error: {}".format(self.__map_name,res_data.message)
                log.LogError(self.GetNodeName(),err_msg) 
        except Exception as error:
            if "trans_floor_service_error" in str(error) :
                # 服务内部错误
                ret_dat["code"]= 0x01200101
                ret_dat["msg"]= "转移楼层服务异常"
            elif "unavailable" in str(error):
                # flask 
                ret_dat["code"]= 0x02200102
                ret_dat["msg"]= "转移楼层服务失效"
            else:
                # flask
                ret_dat["code"]= 0x02000003
                ret_dat["msg"]= "请求失败"
            rospy.logerr("[GoElevtorMap] error: {}".format(error))        
        finally:
            return ret_dat 
    
    def MapInfoSet(self, data):
        '''
            设置地图旋转角度
        '''
        ret_dat = self.ResDataInit()
        client_data = MapManagerRequestRequest()
        client_data.option = MapManagerRequestRequest.SET_ROTATE_ANGLE
        try:
            if not data : raise Exception("Request data error!")
            map_name = data["map_name"].encode('utf-8')
            user_rotate_angle = data["user_rotate_angle"]
            client_data.map_base.name = map_name
            client_data.map_base.rotate_angle = user_rotate_angle
            # 客户端请求
            res_data = self.__map_manager_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message)
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[MapInfoSet] error: {}".format(error))        
        finally:
            return ret_dat 
    

    def AllMapTaskList(self):
        '''
            获取所有的地图及地图下所有任务
        '''
        ret_dat = self.ResDataInit()
        try:
            ret_dat["data"] = []
            task_list_req = TaskSingleManagerRequest()
            task_list_req.option = task_list_req.QUERY_ALL_TASK_INFO
            task_res_data = self.__task_single_client(task_list_req) 
            all_map_task = {}
            for i in range(len(task_res_data.task_list)):
                task_name = task_res_data.task_list[i].task_base.task_name
                map_name = task_res_data.task_list[i].task_base.map_base.name
                if map_name not in all_map_task:
                    all_map_task[map_name] = {
                        "map_name" : map_name,
                        "task_list" : [task_name]
                    }
                else:
                    all_map_task[map_name]["task_list"].append(task_name)
            for task_key, task_item  in all_map_task.items():
                ret_dat["data"].append(task_item)
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[AllMapTaskList] error: {}".format(error))        
        finally:
            return ret_dat 

    #---------------------------------#
    # 重定位操作
    #---------------------------------#
        
    def AreaRelocation(self, data):
        '''
            区域选点重定位
        '''
        ret_dat = self.ResDataInit()
        client_data = RobotRelocationRequestRequest()
        client_data.type = client_data.RELOCATION_AREA
        try:
            if not data : raise Exception("Request data error!")
            client_data.marker_point.pose.position.x = data["point"]['x']
            client_data.marker_point.pose.position.y = data["point"]['y']

            # 客户端请求
            res_data = self.__robot_relocation_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message) 

        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[MapTaskList] error: {}".format(error))        
        finally:
            return ret_dat     


    def RelocationPoint(self, data):
        '''
            标记点重定位
        '''
        ret_dat = self.ResDataInit()
        client_data = RobotRelocationRequestRequest()
        client_data.type = client_data.RELOCATION_POINT
        try:
            if not data : raise Exception("Request data error!")
            if not data["map_name"] : raise Exception("Map name is empty!")
            if not data["tag_name"] : raise Exception("Tag name is empty!")
            client_data.map_base.name = data["map_name"].encode('utf-8')
            client_data.marker_point.name = data["tag_name"].encode('utf-8')

            # 客户端请求
            res_data = self.__robot_relocation_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message) 

        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[MapTaskList] error: {}".format(error))        
        finally:
            return ret_dat     
    
    def RelocationBarCode(self):
        '''反光板重定位
        '''
        ret_dat = self.ResDataInit()
        client_data = RobotRelocationRequestRequest()
        client_data.type = client_data.RELOCATION_REFLECTIVE_BAR
        try:
            # 客户端请求
            res_data = self.__robot_relocation_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message) 
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[MapTaskList] error: {}".format(error))        
        finally:
            return ret_dat    
        
    #---------------------------------#
    # 门禁闸机操作
    #---------------------------------#
    def DoorAccessControlStart(self):
        '''
            门禁标记添加_开始阶段
        '''
        ret_dat = self.ResDataInit()
        client_data = GenAutoGatePathRequest()
        client_data.flag = GenAutoGatePathRequest.START_RECOED
        try:
            res_data = self.__auto_gate_generate_client(client_data)
            if res_data.success:    #此处有歧义，后续修改
                raise Exception(res_data.message) 
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[DoorAccessControlStart] error: {}".format(error))        
        finally:
            return ret_dat
    
    def DoorAccessControlCenter(self):
        '''
            门禁标记添加_中间阶段
        '''
        ret_dat = self.ResDataInit()
        client_data = GenAutoGatePathRequest()
        client_data.flag = GenAutoGatePathRequest.MIDDLE_RECORD
        try: 
            res_data = self.__auto_gate_generate_client(client_data)
            if res_data.success:    #此处有歧义，后续修改
                raise Exception(res_data.message) 
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[DoorAccessControlCenter] error: {}".format(error))        
        finally:
            return ret_dat
    
    def DoorAccessControlEnd(self, data):
        '''
            门禁标记添加_结束保存数据
        '''
        ret_dat = self.ResDataInit()
        client_data = GenAutoGatePathRequest()
        client_data.flag = GenAutoGatePathRequest.SAVE_RECOED
        try:       
            if not data : raise Exception("Request data error!")
            # 解析请求数据
            client_data.auto_gate.map_name = data['map_name'] #地图名
            client_data.auto_gate.gate_type = int(data['gate_type']) #类型， 1--门禁， 2--闸机 3 窄门
            client_data.auto_gate.flag_name = data['flag_name'] #标记名
            client_data.auto_gate.gate_width = data["gate_width"] #门禁宽度，单位：m
            client_data.auto_gate.gate_thick = data["gate_thick"] #门禁宽度，单位：m
            client_data.auto_gate.gate_model_lora_id = data['gate_model_lora_id'] #绑定门禁模组SN
            client_data.auto_gate.gate_model_place_id = data['gate_model_place_id'] # 绑定门禁模组placeid
            client_data.auto_gate.gate_model_lift_id = data['gate_model_lift_id'] #绑定门禁模组deviceid
            client_data.auto_gate.gate_model_lora_id_2 = data['gate_model_lora_id_2'] #绑定门禁模组SN
            client_data.auto_gate.gate_model_place_id_2 = data['gate_model_place_id_2'] # 绑定门禁模组placeid
            client_data.auto_gate.gate_model_lift_id_2 = data['gate_model_lift_id_2'] #绑定门禁模组deviceid
            res_data = self.__auto_gate_generate_client(client_data)
            if res_data.success:    #此处有歧义，后续修改
                raise Exception(res_data.message)
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[DoorAccessControlEnd] error: {}".format(error))        
        finally:
            return ret_dat

    def DoorAccessControlCancel(self):
        '''
            门禁标记添加_取消记录
        '''
        ret_dat = self.ResDataInit()
        client_data = GenAutoGatePathRequest()
        client_data.flag = GenAutoGatePathRequest.CANCEL_RECOED
        try:
            res_data = self.__auto_gate_generate_client(client_data)
            if res_data.success:    #此处有歧义，后续修改
                raise Exception(res_data.message) 
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[DoorAccessControlCancel] error: {}".format(error))        
        finally:
            return ret_dat

    def DoorAccessControlList(self, data):
        '''
            获取门禁闸机列表
        '''
        ret_dat = self.ResDataInit()
        client_data = MapInfoRequestRequest()
        client_data.option = MapInfoRequestRequest.QUERY_MAP_INFO
        try:
            if not data: raise Exception("Request data error!")
            map_name = data["map_name"].encode('utf-8')
            client_data.map_info.base_info.name = map_name
            # 客户端请求
            res_data = self.__map_info_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message) 
            # 解析门禁闸机列表数据
            auto_gates_list = []
            for auto_gate_item in res_data.map_info.auto_gates_list:
                auto_gates_dict = {}
                auto_gates_dict["flag_name"] = auto_gate_item.name
                if auto_gate_item.type == AutoGate.ACCESS_CONTROL:  #门禁
                    auto_gates_dict["gate_type"] = 1
                if auto_gate_item.type == AutoGate.GATE_CONTROL:  #闸机
                    auto_gates_dict["gate_type"] = 2
                if auto_gate_item.type == AutoGate.NARROW_CONTROL:  #窄门
                    auto_gates_dict["gate_type"] = 3
                auto_gates_dict["gate_model_lora_id"] = auto_gate_item.lora.lora_id
                auto_gates_dict["gate_model_place_id"] = auto_gate_item.lora.place_id
                auto_gates_dict["gate_model_lift_id"] = auto_gate_item.lora.lift_id
                auto_gates_dict["gate_model_lora_id_2"] = auto_gate_item.lora_2.lora_id
                auto_gates_dict["gate_model_place_id_2"] = auto_gate_item.lora_2.place_id
                auto_gates_dict["gate_model_lift_id_2"] = auto_gate_item.lora_2.lift_id
                auto_gates_dict["gate_width"] = auto_gate_item.width
                auto_gates_dict["gate_thick"] = auto_gate_item.thick
                auto_gates_dict["waiting_point_a"] = {
                    "x" : auto_gate_item.waiting_point_a.position.x,
                    "y" : auto_gate_item.waiting_point_a.position.y,
                    "z" : auto_gate_item.waiting_point_a.position.z,
                }
                auto_gates_dict["waiting_point_b"] = {
                    "x" : auto_gate_item.waiting_point_b.position.x,
                    "y" : auto_gate_item.waiting_point_b.position.y,
                    "z" : auto_gate_item.waiting_point_b.position.z,
                }
                auto_gates_dict["gate_point"] = {
                    "x" : auto_gate_item.point.position.x,
                    "y" : auto_gate_item.point.position.y,
                    "z" : auto_gate_item.point.position.z,
                }
                auto_gates_list.append(auto_gates_dict)
            ret_dat["data"] = auto_gates_list   
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[DoorAccessControlList] error: {}".format(error))        
        finally:
            return ret_dat        

    def DoorAccessControlDel(self,data):
        '''
            删除门禁闸机
        '''
        ret_dat = self.ResDataInit()
        client_data = MapInfoRequestRequest()
        client_data.option = MapInfoRequestRequest.DELETE_AUTOGATE
        try:
            if not data: raise Exception("Request data error!")
            map_name = data["map_name"].encode('utf-8')
            flag_name = data["flag_name"].encode('utf-8')
            # 构造请求数据
            auto_gate_msg = AutoGate()
            auto_gate_msg.name = flag_name
            client_data.map_info.base_info.name = map_name
            client_data.map_info.auto_gates_list.append(auto_gate_msg)
            # 客户端请求
            res_data = self.__map_info_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message) 
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[DoorAccessControlDel] error: {}".format(error))        
        finally:
            return ret_dat 

    def DoorAccessControlEdit(self,data):
        '''
            更新门禁闸机
        '''
        ret_dat = self.ResDataInit()
        client_data = MapInfoRequestRequest()
        client_data.option = MapInfoRequestRequest.UPDATE_AUTOGATE
        try:
            if not data: raise Exception("Request data error!")
            map_name = data["map_name"].encode('utf-8')
            flag_name = data["flag_name"].encode('utf-8')
            # 构造auto_gate_msg
            auto_gate_msg = AutoGate()
            auto_gate_msg.name = flag_name;            
            # 构造new_auto_gate_msg
            new_auto_gate_msg = AutoGate()
            new_auto_gate_msg.name = data["flag_name_new"].encode('utf-8')
            new_auto_gate_msg.lora.lora_id = data["gate_model_lora_id"]
            new_auto_gate_msg.lora.place_id = data["gate_model_place_id"]
            new_auto_gate_msg.lora.lift_id = data["gate_model_lift_id"] 
            new_auto_gate_msg.lora_2.lora_id = data["gate_model_lora_id_2"]
            new_auto_gate_msg.lora_2.place_id = data["gate_model_place_id_2"]
            new_auto_gate_msg.lora_2.lift_id = data["gate_model_lift_id_2"]
            new_auto_gate_msg.thick = data["gate_thick"]
            new_auto_gate_msg.width = data["gate_width"]           
            # 构造client_data
            client_data.map_info.base_info.name = map_name
            client_data.map_info.auto_gates_list.append(auto_gate_msg)
            client_data.new_map_info.auto_gates_list.append(new_auto_gate_msg)         
            # 客户端请求
            res_data = self.__map_info_client.call(client_data)
            if not res_data.response.success:
                raise Exception(res_data.response.message) 
        except Exception as error:
            ret_dat["code"]= -1
            ret_dat["msg"]= str(error)
            rospy.logerr("[DoorAccessControlEdit] error: {}".format(error))        
        finally:
            return ret_dat 


